#pragma config(Sensor, dgtl1,  suckSensor,          sensorTouch)
#pragma config(Sensor, dgtl2,  liftUpSensor,        sensorTouch)
#pragma config(Sensor, dgtl3,  btnLiftDown,         sensorTouch)
#pragma config(Sensor, dgtl4,  btnBackL,            sensorTouch)
#pragma config(Sensor, dgtl5,  btnBackR,            sensorTouch)
#pragma config(Sensor, dgtl6,  btnFrontL,           sensorTouch)
#pragma config(Sensor, dgtl7,  btnFrontR,           sensorTouch)
#pragma config(Sensor, dgtl8,  encL,                sensorQuadEncoder)
#pragma config(Sensor, dgtl10, encR,                sensorQuadEncoder)
#pragma config(Motor,  port1,           rightF,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port2,           rightUp,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           rightLow,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port4,           suckR,         tmotorNormal, openLoop)
#pragma config(Motor,  port5,           suckL,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port6,           liftR,         tmotorNormal, openLoop)
#pragma config(Motor,  port7,           liftL,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port8,           leftLow,       tmotorNormal, openLoop)
#pragma config(Motor,  port9,           leftUp,        tmotorNormal, openLoop)
#pragma config(Motor,  port10,          leftF,         tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

/*INCLUDES*/
#include "Vex_Competition_Includes.c"
#include "zeta_lib.c"

/*DEFINITIONS*/
#define FULL 127

/*PRE AUTONOMOUS*//////////////////////////////////////////////////////////////
void pre_auton() {}

/*AUTONOMOUS*//////////////////////////////////////////////////////////////////
task autonomous() {
  //drive to low goal
  drive_btn(FULL, -1);
  //score preload
  suck_msec(-FULL,900);
  //return home
  drive_forward_msec(FULL, 600);

  /////////Insert button control here///////////
  ///User loads 4 and lines up with middle tower///

  //drive to tower
  drive_btn(FULL, -1);
  //raise lift
  lift_up(FULL);
  //spit out/score
  suck_msec(-FULL,900);
  //lower lift
  lift_down(FULL);
  //Drive home
  drive_forward_msec(FULL, 2000);
}//Auton

/*TELE OP*/////////////////////////////////////////////////////////////////////
task usercontrol() {
  while (true) {

    // drive
    setDriveLSpeed(vexRT[Ch3]);
    setDriveRSpeed(vexRT[Ch2]);

    //  suck
    setSuckLSpeed((vexRT[Btn5U] - vexRT[Btn5D])*FULL);
    setSuckRSpeed((vexRT[Btn6U] - vexRT[Btn6D])*FULL);


    //auto raise suck
    if ((vexRT[Btn8U]) == 1) {
      if (SensorValue[suckSensor] == 0) {
        setSuckSpeed(FULL);
      }
      else {
        killSuck();
      }
    }//if

    //raise lift
    setLiftSpeed((vexRT[Btn7U] - vexRT[Btn7D])*FULL);

    //auto raise lift
    if ((vexRT[Btn7L]) == 1) {
      if (SensorValue[liftUpSensor] == 0) {
        setLiftSpeed(FULL);
      }
      else {
        killLift();
      }
    }

    //auto lower lift
    if ((vexRT[Btn7R]) == 1) {
      if (SensorValue[liftDownSensor] == 0) {
        setLiftSpeed(FULL);
      }
      else {
        killLift();
      }
    }

  }//WHILE
}//TELEOP

/*END*/
